/*
 * cmd_efuse_simple_test.c
 *
 *  Created on: Nov 10, 2016
 *      Author: duobao
 */


/*
 * cmd_efuse_test.c
 *
 *  Created on: Sep 28, 2016
 *      Author: duobao
 */

#include "../drivers/crypto/fh_aes.h"
#include <common.h>
#include <command.h>

//#define FH_EFUSE_TOOLS_DEBUG printf
#define FH_EFUSE_TOOLS_DEBUG(...)

#define _MAP_MODE_INIT(c,name)     {.no = c, .string = (char *)name,}
#define _FIND_MAP_STRING(p_map,no)     p_map[no].string

struct CRYPTO_WORK_MODE_MAP
{
    int no;
    char *string;
};

enum
{
    SECURE_BOOT_ECB = 0,
    SECURE_BOOT_CBC = 1,
    SECURE_BOOT_CTR = 2,
    SECURE_BOOT_CFB = 4,
    SECURE_BOOT_OFB = 5,
};

static struct CRYPTO_WORK_MODE_MAP work_mode_map[] =
{
    _MAP_MODE_INIT(SECURE_BOOT_ECB, "ecb"),
    _MAP_MODE_INIT(SECURE_BOOT_CBC, "cbc"),
    _MAP_MODE_INIT(SECURE_BOOT_CTR, "ctr"),
    _MAP_MODE_INIT(SECURE_BOOT_CFB, "cfb"),
    _MAP_MODE_INIT(SECURE_BOOT_OFB, "ofb"),
};

#define EFUSE_MAX_ENTRY_SIZE	64
#define SECURE_BOOT_EFUSE_ENTRY	48
#define SECURE_KEY_EFUSE_ENTRY	0
#define SECURE_IV_EFUSE_ENTRY	16
#define SECURE_KEY_EFUSE_SIZE	16
#define SECURE_IV_EFUSE_SIZE	16
#define EFUSE_MATCH_KEY	0x92fc0025

struct efuse_head {
	unsigned int chipid;
	unsigned int data_cnt;
	unsigned int filesize;
	unsigned int reserved[5];
};

struct efuse_data_head {
	unsigned int startentry;
	unsigned int datasize;
	unsigned int readbackcheck;
	unsigned int reserved[5];
};

#define _MAP_MODE_INIT(c,name)     {.no = c, .string = (char *)name,}




char * map_val_to_string(struct CRYPTO_WORK_MODE_MAP *p, int size, int val){
	int i;

	for(i=0; i < size; i++){
		if(p[i].no == val)
			return p[i].string;
	}
	return 0;
}

static void aes_biglittle_swap(u8 *buf) {
	u8 tmp, tmp1;
	tmp = buf[0];
	tmp1 = buf[1];
	buf[0] = buf[3];
	buf[1] = buf[2];
	buf[2] = tmp1;
	buf[3] = tmp;
}
int cmd_aes_crypto_demo(unsigned int src,unsigned int dst,unsigned int len)
{

	struct rt_crypto_obj *crypto_obj;
	char crypto_name[20];
	u32 *p_dst = NULL;
	int i;
	char* sec_boot_name;
	struct wrap_efuse_obj *p_efuse;
	int ret;
//	unsigned char key_buf[SECURE_KEY_EFUSE_SIZE];
	unsigned char iv_buf[SECURE_IV_EFUSE_SIZE];
	unsigned char secure_boot_val;
	struct rt_crypto_request request;

	p_efuse = (struct wrap_efuse_obj *)get_efuse_handle();
	memset(&request, 0 , sizeof(struct rt_crypto_request));
	
	refresh_efuse(p_efuse);
	efuse_read_entry(p_efuse, EFUSE_MATCH_KEY, SECURE_BOOT_EFUSE_ENTRY, &secure_boot_val, 1);
	secure_boot_val = secure_boot_val >> 1;
	secure_boot_val &= 0x7;
	sec_boot_name = map_val_to_string(work_mode_map, sizeof(work_mode_map)/sizeof(struct CRYPTO_WORK_MODE_MAP), secure_boot_val);
	if(!sec_boot_name){
		printf("can't find  secure_boot_val [%x]\n",secure_boot_val);
		return -1;
	}
	sprintf(crypto_name, "%s%s%s", "aes", "-", sec_boot_name);
	/* find crypto mode..*/
	crypto_obj = rt_algo_obj_find(RT_Algo_Class_Crypto, crypto_name);
	if(!crypto_obj) {
		printf("can't find  crypto name : [%s]\n",crypto_name);
		return -1;
	}
	/* get iv..*/
	refresh_efuse(p_efuse);
	efuse_read_entry(p_efuse, EFUSE_MATCH_KEY, SECURE_IV_EFUSE_ENTRY, iv_buf, sizeof(iv_buf));
	p_dst = (u32 *)iv_buf;
	for (i = 0; i < SECURE_IV_EFUSE_SIZE / sizeof(u32); i++)
		aes_biglittle_swap((u8 *) (p_dst + i));

	/* go...*/
	request.data_src = (unsigned char *)src;
	request.data_dst = (unsigned char *)dst;
	request.data_size = len;
	request.key_size = SECURE_KEY_EFUSE_SIZE;
	request.iv = (unsigned char *)iv_buf;
	request.iv_size = sizeof(iv_buf);
	request.key_flag &= ~(AES_FLAG_SUPPORT_CPU_SET_KEY | AES_FLAG_SUPPORT_EFUSE_SET_KEY);
	request.key_flag |= AES_FLAG_SUPPORT_EFUSE_SET_KEY;

	ret = rt_algo_crypto_setkey(crypto_obj, &request);
	if (0 != ret)
	{
		printf("[crypt_api]: crypt_cipher_crypt failed, ret=%d\n", ret);
		return ret;
	}

	ret = rt_algo_crypto_decrypt(crypto_obj, &request);
	if (0 != ret)
	{
		printf("[crypt_api]: crypt_cipher_crypt failed, ret=%d\n", ret);
		return ret;
	}
	return 0;
}
#include "rsa.h"
#define KEY_ADDR 0xa3000000
static int VerifySignature(
    const unsigned char *code_address,unsigned int code_size,
    const unsigned char *encrypted_sign_addr)
{
	int ret;
	mbedtls_rsa_context rsa;
	unsigned int rsa_e = RSA_E;
	unsigned char chksum[32]={0};

	mbedtls_rsa_init( &rsa, MBEDTLS_RSA_PKCS_V15, 0 );

	mbedtls_mpi_read_binary( &rsa.N, KEY_ADDR, 256 );
	mbedtls_mpi_read_binary( &rsa.E, (const unsigned char *)&rsa_e, 4 );


	ret = mbedtls_rsa_check_pubkey(&rsa);
	if(ret < 0)
	{
		printf("public key err  \n");
		return ret;
	}

	rsa.len = mbedtls_mpi_size(&rsa.N);

	
#ifdef CRYPTO_HASH_METHOD_SHA1
	mbedtls_sha1( (const unsigned char *)code_address, code_size, chksum );
#elif defined(CRYPTO_HASH_METHOD_SHA256)
	mbedtls_sha256( (const unsigned char *)code_address, code_size, chksum,0);
#endif

	if( ( ret = mbedtls_rsa_pkcs1_verify( &rsa, chksum, (const unsigned char *)encrypted_sign_addr ) ) != 0 )
	{
		printf("mbedtls_rsa_pkcs1_verify failed %d\n",ret);
		return ret;
	} else {
		printf("verify ok \n");
	}

	return 0;
}
typedef struct {
    char name[16];
    unsigned int padding_size;
    unsigned int origin_size;
    unsigned int signaturesize;
    unsigned int shalen;
}fh_secure_bin_head;

int aes_decrypt_fh_uImage(cmd_tbl_t *cmdtp,int flag,int argc,char *argv[])
{
	unsigned int src;
	unsigned int dst;
	unsigned int originsize;
	unsigned int wholesize;
	fh_secure_bin_head* head = NULL;
	if (argc != 3) {
		cmd_usage(cmdtp);
		return -1;
	}

	src = simple_strtoul(argv[1], NULL, 16);
	dst = simple_strtoul(argv[2], NULL, 16);
	head = (fh_secure_bin_head*)src;
	
	wholesize = head->padding_size;
	originsize = head->origin_size;    
	cmd_aes_crypto_demo(src+sizeof(fh_secure_bin_head),dst,wholesize);
	VerifySignature((const unsigned char*)(dst+256),originsize,dst);

	return 0;
}

U_BOOT_CMD(

	aes_decrypt_fh_uImage,5,1,aes_decrypt_fh_uImage,"usage:aes_decrypt_fh_uImage\n","src dest \n"

);
